/*
 * @Author Hien Hoang
 * @Version Nov 26, 2013
 * */

#include "WSNIPv4.h"
#include "WSNIPProtocolld.h"
#include "WSNNotifierConsts.h"
#include "IPv4.h"
#include "ARPPacket_m.h"
#include "ICMPMessage_m.h"
#include "InterfaceTableAccess.h"
#include "IPv4ControlInfo.h"
#include "IPv4Datagram.h"
#include "IPv4InterfaceData.h"
#include "IRoutingTable.h"
#include "Ieee802Ctrl_m.h"
#include "NodeOperations.h"
#include "NodeStatus.h"
#include "NotificationBoard.h"

Define_Module(WSNIPv4);

void WSNIPv4::initialize(int stage)
{
    if (stage == 0)
    {
        QueueBase::initialize();

        numForward = 0;
        numSent = 0;
        numDropped = 0;
        curFragmentId = 0;
        queueOutGate = gate("queueOut");
        mapping.parseProtocolMapping(par("protocolMapping"));


    }
    else if (stage == 1)
    {
        isUp = true;
        ift = InterfaceTableAccess().get();
        rt = RoutingTableAccess().get();
        nb = NotificationBoardAccess().getIfExists();
        nb->subscribe(this, BATTERY_RESIDUAL_EMPTY);

    }
}

void WSNIPv4 :: endService(cPacket *msg){

    if (!isUp) {
            EV << "WSNIPv4 is down -- discarding message\n";
            delete msg;
            return;
    }
    if (msg->getArrivalGate()->isName("protocolIn"))
    {
        EV << "[WSNIP]! Got a message from higher" << endl;
        handleMessageFromHigher( msg );
    }
    else if (dynamic_cast<ARPPacket *>(msg))
    {
        // dispatch ARP packets to ARP
        handleARP((ARPPacket *)msg);
    }
    else
    {
        EV << "[WSNIP]! Got a message from lower" << endl;
        IPv4Datagram *dgram = check_and_cast<IPv4Datagram *>(msg);
        InterfaceEntry *fromIE = getSourceInterfaceFrom(dgram);
        handlePacketFromLower(dgram, fromIE);
    }

}

void WSNIPv4 :: handlePacketFromLower(IPv4Datagram *datagram, InterfaceEntry *fromIE){

    EV << "[WSNIP]! handlePacketFromLower processing... " << endl;
    int protocol = datagram->getTransportProtocol();
    IPv4Address &destAddr = datagram->getDestAddress();

    if (destAddr.isMulticast()) {
        // If the destAddress is multicast, so datagram is send for me too
        // set the destination address is this hop address
        destAddr = fromIE->ipv4Data()->getIPAddress();
        datagram->setDestAddress(destAddr);
    }

    if (destAddr != fromIE->ipv4Data()->getIPAddress()){

        // If the destination address is not me, also not multicast, so route it to other hop
        EV << "[WSNIP]! Routing packet..." << endl;
        numForward++;
        routePacket(datagram, fromIE, IPv4Address::UNSPECIFIED_ADDRESS);
    }
    else {

        // OK, this datagram is for me, let's proccess it
        EV << "[WSNIP]! Received packet '" << datagram->getName() << "' with dest=" << destAddr << "\n";

        if (datagram->getTimeToLive() == 0) {

            EV << "[WSNIP]! time to live reached 0, dropping packet.\n";
            delete datagram;
            return;
        }

        if (protocol == IP_PROT_LBTR) {

            EV << "[WSNIP]! Passing packet to LBTR routing protocol !!!" << endl;
            sendToRouting(datagram);
        }
        if (protocol == IP_PROT_LBAPP) {

            EV << "[WSNIP]! Passing packet to LBAPP application protocol" << endl;
            sendToUpper(datagram);
        }
    }

}

void WSNIPv4 :: handleMessageFromHigher(cPacket *msg) {

    // if no interface exists, do not send datagram
    if (ift->getNumInterfaces() == 0) {

        EV << "No interfaces exist, dropping packet\n";
        numDropped++;
        delete msg;
        return;
    }

    // encapsulate and send
    IPv4Datagram *datagram = dynamic_cast<IPv4Datagram *>(msg);
    IPv4ControlInfo *controlInfo = NULL;
    InterfaceEntry *ie = ift->getInterfaceByName("wlan0");

    EV << "[WSNIP]! I'm " << ie->ipv4Data()->getIPAddress() << endl;
    if (datagram) { // if Higher sends an IPv4Datagram, it mean send from routing protocol
        EV << "[WSNIP]! This is a packet send from routing to " << datagram->getDestAddress() << endl;
        if (datagram->getSrcAddress() == IPv4Address::UNSPECIFIED_ADDRESS) {

            datagram->setSrcAddress(ie->ipv4Data()->getIPAddress());
        }

    }
    else
    {
        // encapsulate
        controlInfo = check_and_cast<IPv4ControlInfo*>(msg->removeControlInfo());
        datagram = encapsulate(msg, controlInfo);
        datagram->setSrcAddress(ie->ipv4Data()->getIPAddress());
        numSent++;
    }
    if (par("isDisplayNodeString").boolValue()) {
        updateNodeDisplay();
    }
    routePacket(datagram, ie, IPv4Address::UNSPECIFIED_ADDRESS);
}

void WSNIPv4 :: routePacket(IPv4Datagram *datagram, InterfaceEntry *destIE, IPv4Address destNextHopAddr){

    IPv4Address destAddr = datagram->getDestAddress();
    const IPv4Route *re = rt->findBestMatchingRoute(destAddr);
    IPv4Address nextHopAddr;
    // if destination interface and destination next hop address is specified, it mean
    // route it with this information, otherwise find it
    if (destIE) {
        if (!destNextHopAddr.isUnspecified()){
            nextHopAddr = destNextHopAddr;
        }
        else {
            // Let's find destAddr in neighbor, if not found, choose my best route
            bool found = false;
            for (int i = 0; i < rt->getNumRoutes(); i++) {
                IPv4Route *route = rt->getRoute(i);
                if (route->getGateway() == destAddr) {
                    found = true;
                    nextHopAddr = route->getGateway();
                }
            }
            if (!found && re) {
                nextHopAddr = rt->findBestMatchingRoute(destAddr)->getGateway();
            }
        }
    }
    else {

        if (re)
        {
            destIE = re->getInterface();
            nextHopAddr = re->getGateway();
        }
    }

    if (datagram->getSrcAddress().isUnspecified())
        datagram->setSrcAddress(destIE->ipv4Data()->getIPAddress());

    datagram->setTimeToLive(datagram->getTimeToLive()-1);

    EV << "[WSNIP]! Sending datagram to next hop [" << nextHopAddr << "]....." << endl;
    sendDatagramToOutput(datagram, destIE, nextHopAddr);

}

void WSNIPv4 :: handleARP(ARPPacket *msg)
{
    // delete old control info
    delete msg->removeControlInfo();

    // dispatch ARP packets to ARP and let it know the gate index it arrived on
    InterfaceEntry *fromIE = getSourceInterfaceFrom(msg);
    IPv4RoutingDecision *routingDecision = new IPv4RoutingDecision();
    routingDecision->setInterfaceId(fromIE->getInterfaceId());
    msg->setControlInfo(routingDecision);

    send(msg, queueOutGate);
}

IPv4Datagram *WSNIPv4 :: encapsulate(cPacket *protocolPacket, IPv4ControlInfo *controlInfo)
{
    IPv4Datagram *datagram = createIPv4Datagram(protocolPacket->getName());
    datagram->setByteLength(IP_HEADER_BYTES);
    datagram->encapsulate(protocolPacket);

    // set source and destination address
    IPv4Address dest = controlInfo->getDestAddr();
    datagram->setDestAddress(dest);

    IPv4Address src = controlInfo->getSrcAddr();

    // when source address was given, use it; otherwise it'll get the address
    // of the outgoing interface after routing
    if (!src.isUnspecified())
    {
        // if interface parameter does not match existing interface, do not send datagram
        if (rt->getInterfaceByAddress(src)==NULL)
            throw cRuntimeError("Wrong source address %s in (%s)%s: no interface with such address",
                      src.str().c_str(), protocolPacket->getClassName(), protocolPacket->getFullName());

        datagram->setSrcAddress(src);
    }

    // set other fields
    datagram->setTypeOfService(controlInfo->getTypeOfService());

    datagram->setIdentification(curFragmentId++);
    datagram->setMoreFragments(false);
    datagram->setDontFragment(controlInfo->getDontFragment());
    datagram->setFragmentOffset(0);

    short ttl;
    if (controlInfo->getTimeToLive() > 0)
        ttl = controlInfo->getTimeToLive();
    else if (datagram->getDestAddress().isLinkLocalMulticast())
        ttl = 1;
    else if (datagram->getDestAddress().isMulticast())
        ttl = defaultMCTimeToLive;
    else
        ttl = defaultTimeToLive;
    datagram->setTimeToLive(ttl);
    datagram->setTransportProtocol(controlInfo->getProtocol());

    // setting IPv4 options is currently not supported

    return datagram;
}

cPacket *WSNIPv4::decapsulate(IPv4Datagram *datagram)
{
    // decapsulate transport packet
    InterfaceEntry *fromIE = getSourceInterfaceFrom(datagram);
    cPacket *packet = datagram->decapsulate();

    // create and fill in control info
    IPv4ControlInfo *controlInfo = new IPv4ControlInfo();
    controlInfo->setProtocol(datagram->getTransportProtocol());
    controlInfo->setSrcAddr(datagram->getSrcAddress());
    controlInfo->setDestAddr(datagram->getDestAddress());
    controlInfo->setTypeOfService(datagram->getTypeOfService());
    controlInfo->setInterfaceId(fromIE ? fromIE->getInterfaceId() : -1);
    controlInfo->setTimeToLive(datagram->getTimeToLive());

    // original IPv4 datagram might be needed in upper layers to send back ICMP error message
    controlInfo->setOrigDatagram(datagram);

    // attach control info
    packet->setControlInfo(controlInfo);

    return packet;
}

void WSNIPv4::sendDatagramToOutput(IPv4Datagram *datagram, InterfaceEntry *ie, IPv4Address nextHopAddr)
{
    {
        // send out datagram to ARP, with control info attached
        delete datagram->removeControlInfo();
        IPv4RoutingDecision *routingDecision = new IPv4RoutingDecision();
        routingDecision->setInterfaceId(ie->getInterfaceId());
        routingDecision->setNextHopAddr(nextHopAddr);
        datagram->setControlInfo(routingDecision);

        send(datagram, queueOutGate);
    }
}

void WSNIPv4 :: sendToRouting(IPv4Datagram *datagram) {

    int gateindex = mapping.getOutputGateForProtocol(datagram->getTransportProtocol());
    send(datagram, "protocolOut", gateindex);
}

void WSNIPv4 :: sendToUpper(IPv4Datagram *datagram) {

    int gateindex = mapping.getOutputGateForProtocol(datagram->getTransportProtocol());
    send(decapsulate(datagram), "protocolOut", gateindex);
}

InterfaceEntry *WSNIPv4 :: getSourceInterfaceFrom(cPacket *msg){

    cGate *g = msg->getArrivalGate();
    return g ? ift->getInterfaceByNetworkLayerGateIndex(g->getIndex()) : NULL;
}

IPv4Datagram *WSNIPv4::createIPv4Datagram(const char *name)
{
    return new IPv4Datagram(name);
}

void WSNIPv4 :: updateNodeDisplay() {
    if (getParentModule()->getParentModule()->isEvEnabled()) {
       char buf[40];
       sprintf(buf, "S: %d - Fw: %d", numSent, numForward);
       getParentModule()->getParentModule()->getDisplayString().setTagArg("t",0,buf);
   }
}

void WSNIPv4 :: receiveChangeNotification(int category, const cObject *details) {
    if (category == BATTERY_RESIDUAL_EMPTY && isUp == true) {

        EV << "Battery exhausted, WSNIPv4 module going down... !!!" << endl;
        isUp = false;
    }
}


